%% Saturn
warning off
clc;clear;close all

format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.



%% environment settings
dt = 0.01;
startphase=0; % changing the starting position of the periodic
% orbit to show changes in the pole visibility based on the Satellite's position wrt the Sun's pole
% OpenCRTBP_u(u)
%% Define inertial frame FNs
[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,180);
figure;
s = surface(xs,ys,zs);
rotate(s, [1 0 0], 7.25);
rotate(s, [0 0 1], 73.67+0.014*(2023-1850));
InclinationRotationAngle = 7.25;
RAANRotationAngle = 73.67+0.014*(2023-1850);
PoleDirection = Rotation([0;0;1],RAANRotationAngle,'Degrees')*...
    Rotation([1;0;0],InclinationRotationAngle,'Degrees')*...
    [0;0;1];
rotate(s, PoleDirection, -70);
FN_i = s.VertexNormals;
[row,col,~] = size(FN_i);
%%
load('SE_L4_Vertical.mat');
load('SE_L5_Vertical.mat');
inc_list = zeros(1,length(T_L4_VL));
for ii = 1:length(T_L4_VL)
    IC = IC_L4_VL(:,ii);
    T = T_L4_VL(ii);
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:T],IC,options);
    S = S';
    S_i = zeros(size(S));
    S_i = C2I_primary(S(:,ii),u,DU,VU,t(ii));
    COE_list = State2Coe(S_i,GM_central);
    inc_list(ii) = COE_list(3);
end
%%
load('SEL1_Lyapunov.mat');
whichoneL1 = 1;
[t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:10*2*pi],IC(:,whichoneL1),options);
S=S';
S_i = zeros(6,length(t));
for ii = 1:length(t)
    S_i(:,ii) = C2I_primary(S(:,ii),u,DU,VU,t(ii));
    epoch(ii) = datetime(2023,1,1,0,0,0) + seconds(t(ii)*TU);
end
L1.S_i = S_i;
L1.epoch = epoch;

tlength = length(L1.epoch);

%% Given orbital states of the satellite (varying inclination), allowable theta_max, and target latitude,
%  Determine visibility

CameraAngleCapability=60;
target_latitude = -90:90;
orbit_amount = 50;
A=14.713;
B=-2.396;
C=-1.787;
DIFFDRIFT = A+B*sind(target_latitude).^2+C*sind(target_latitude).^4;
drift_dt = 0.593721739974254;
%%
tic
for iii = 1:length(CameraAngleCapability)
    fprintf('CameraAngleCapability=%d \n',CameraAngleCapability(iii))
    ind = 1;
    for n = [182,500]

        fprintf('%d / %d \n',n,length(IC_L4_VL))
        L4 = DetermineOptimalL4andL5State_10yr(IC_L4_VL(:,n),T_L4_VL(n),u,DU,VU,TU,dt);
        L5 = DetermineOptimalL4andL5State_10yr(IC_L5_VL(:,n),T_L4_VL(n),u,DU,VU,TU,dt);
        visibilityCondition_L4=zeros(length(target_latitude),length(L4.epoch));
        visibilityCondition_L5=zeros(length(target_latitude),length(L4.epoch));
        visibilityCondition_L1=zeros(length(target_latitude),length(L4.epoch));
        visibilityCondition_L1_imp=zeros(length(target_latitude),length(L4.epoch));
        for j = 1:length(target_latitude)
            SunSpotPosition=reshape([FN_i(91+target_latitude(j),91,:)],3,1);
            for k = 1:tlength
                RotationMatrix = Rotation(PoleDirection,DIFFDRIFT(j)*(k-1)*(drift_dt),'degrees');
                SunSpotPosition_now=RotationMatrix*SunSpotPosition;
                



                posvec_L1 = L4.S_i(1:3,k);
                angle_temp_L1 = acosd(dot(SunSpotPosition_now,posvec_L1)/(norm(SunSpotPosition_now)*norm(posvec_L1)));
                if angle_temp_L1<CameraAngleCapability(iii)
                    visibilityCondition_L1_imp(j,k) = 1;
                else
                    visibilityCondition_L1_imp(j,k) = 0;
                end

                posvec_L4 = L4.S_i(1:3,k);
                angle_temp_L4 = acosd(dot(SunSpotPosition_now,posvec_L4)/(norm(SunSpotPosition_now)*norm(posvec_L4)));
                if angle_temp_L4<CameraAngleCapability(iii)
                    visibilityCondition_L4(j,k) = 1;
                    continue
                else
                    visibilityCondition_L4(j,k) = 0;
                end

                posvec_L5 = L5.S_i(1:3,k);
                angle_temp_L5 = acosd(dot(SunSpotPosition_now,posvec_L5)/(norm(SunSpotPosition_now)*norm(posvec_L5)));
                if angle_temp_L5<CameraAngleCapability(iii)
                    visibilityCondition_L5(j,k) = 1;
                    continue
                else
                    visibilityCondition_L5(j,k) = 0;
                end

                posvec_L1 = L1.S_i(1:3,k);
                angle_temp_L1 = acosd(dot(SunSpotPosition_now,posvec_L1)/(norm(SunSpotPosition_now)*norm(posvec_L1)));
                if angle_temp_L1<CameraAngleCapability(iii)
                    visibilityCondition_L1(j,k) = 1;
                    continue
                else
                    visibilityCondition_L1(j,k) = 0;
                end

            end
        end
        A = visibilityCondition_L1+visibilityCondition_L4+...
            visibilityCondition_L5;
        for ii = 1:size(A,1)
            A_temp = A(ii,:);
            d1=~diff(A_temp).*(1:numel(A_temp)-1);
            idx1=d1.*[1  ~d1(1:end-1)];
            idx2=circshift(d1.*[~d1(2:end) 1],[0 1]);
            ii1=~~idx1;
            out=[A_temp(ii1);idx1(ii1) ;idx2(~~idx2)+1];
            ind_ones = find(out(1,:) == 1);
            %%%
%             for ii = 1:size(A,1)
%                 temp(ii) = sum(A(ii,:));
%             end
            
            %%%
            if sum(out(1,:) == 1)<2
                mean_observ_threesat(ii)=nan;
            else
                if out(1,1)==0
                    ind_time = 1;
                    for k = ind_ones(1:end-1)
                        dt_observ(ind_time) = (out(2,k+1)-out(2,k));
                        ind_time=ind_time+1;
                    end
                    mean_observ_threesat(ii)=sum(dt_observ)*dt*TU/3600/24*(1/10)/12/30*DIFFDRIFT(ii);
                else
                    ind_time = 1;
                    for k = ind_ones(1:end-1)
                        dt_observ(ind_time)= (out(2,k+1)-out(2,k));
                        ind_time=ind_time+1;
                    end
                    mean_observ_threesat(ii)=sum(dt_observ)*dt*TU/3600/24*(1/10)/12/30*DIFFDRIFT(ii);
                end
                clear dt_observ
            end
        end
        A2 = visibilityCondition_L1_imp;
        for ii = 1:size(A2,1)
            A_temp = A2(ii,:);
            d1=~diff(A_temp).*(1:numel(A_temp)-1);
            idx1=d1.*[1  ~d1(1:end-1)];
            idx2=circshift(d1.*[~d1(2:end) 1],[0 1]);
            ii1=~~idx1;
            out=[A_temp(ii1);idx1(ii1) ;idx2(~~idx2)+1];
            ind_ones = find(out(1,:) == 1);
            if sum(out(1,:) == 1)<2
                mean_observ_onesat(ii)=nan;
            else
                if out(1,1)==0
                    ind_time = 1;
                    for k = ind_ones(1:end-1)
                        dt_observ(ind_time) = (out(2,k+1)-out(2,k));
                        ind_time=ind_time+1;
                    end
                    mean_observ_onesat(ii)=sum(dt_observ)*dt*TU/3600/24*(1/10)/12/30*DIFFDRIFT(ii);
                else
                    ind_time = 1;
                    for k = ind_ones(1:end-1)
                        dt_observ(ind_time)= (out(2,k+1)-out(2,k));
                        ind_time=ind_time+1;
                    end
                    mean_observ_onesat(ii)=sum(dt_observ)*dt*TU/3600/24*(1/10)/12/30*DIFFDRIFT(ii);
                end
                clear dt_observ
            end
        end

        A2 = visibilityCondition_L1+visibilityCondition_L4;
        for ii = 1:size(A2,1)
            A_temp = A2(ii,:);
            d1=~diff(A_temp).*(1:numel(A_temp)-1);
            idx1=d1.*[1  ~d1(1:end-1)];
            idx2=circshift(d1.*[~d1(2:end) 1],[0 1]);
            ii1=~~idx1;
            out=[A_temp(ii1);idx1(ii1) ;idx2(~~idx2)+1];
            ind_ones = find(out(1,:) == 1);
            if sum(out(1,:) == 1)<2
                mean_observ_twosat(ii)=nan;
            else
                if out(1,1)==0
                    ind_time = 1;
                    for k = ind_ones(1:end-1)
                        dt_observ(ind_time) = (out(2,k+1)-out(2,k));
                        ind_time=ind_time+1;
                    end
                    mean_observ_twosat(ii)=sum(dt_observ)*dt*TU/3600/24*(1/10)/12/30*DIFFDRIFT(ii);
                else
                    ind_time = 1;
                    for k = ind_ones(1:end-1)
                        dt_observ(ind_time)= (out(2,k+1)-out(2,k));
                        ind_time=ind_time+1;
                    end
                    mean_observ_twosat(ii)=sum(dt_observ)*dt*TU/3600/24*(1/10)/12/30*DIFFDRIFT(ii);
                end
                clear dt_observ
            end
        end

        onesat_save{iii,ind}=mean_observ_onesat;
        twosat_save{iii,ind}=mean_observ_twosat;
        threesat_save{iii,ind} = mean_observ_threesat;
        ind = ind +1;
        findfigs
    end
end
toc


%%
% figure;
% hold on;
% x=-10;
% for ii = 1:3
% plot(target_latitude,1.8*onesat_save{ii,1},'ko-','MarkerIndices',1:10:181)
% plot(target_latitude,1.5*threesat_save{ii,1},'ks--','MarkerIndices',1:10:181)
% text_list = sprintf('θ = %d',CameraAngleCapability(ii));
% y = max(1.5*threesat_save{ii,1})-0.5;
% text(x,y,text_list);
% y = max(1.8*onesat_save{ii,1})-0.53;
% text(x,y,text_list);
% end
% xlabel('Latitude','Interpreter','latex')
% ylabel('$\frac{Visible}{rev(\lambda)}$','Interpreter','latex')
% set(gca,'fontsize',15)
% legend('1-sat','3-sat')
% title('Inclination=14.5deg')
% xlim([-90,90])
%%
% figure;
% hold on;
% for ii = 1:3
% plot(target_latitude,1.8*onesat_save{ii,2},'ko-','MarkerIndices',1:10:181)
% plot(target_latitude,1.5*threesat_save{ii,2},'ks--','MarkerIndices',1:10:181)
% text_list = sprintf('θ = %d',CameraAngleCapability(ii));
% y = max(1.5*threesat_save{ii,1})-0.5;
% text(x,y,text_list);
% y = max(1.8*onesat_save{ii,1})-0.5;
% text(x,y,text_list);
% end
% legend('1-sat','3-sat')
% xlabel('Latitude','Interpreter','latex')
% ylabel('$\frac{Visible}{rev(\lambda)}$','Interpreter','latex')
% set(gca,'fontsize',15)
% title('Inclination=0deg')
% xlim([-90,90])
%%
% markerSize = 9;
% figure;
% hold on;
% x=-10;
% for ii = 1
% plot(target_latitude,1.6*onesat_save{ii,1},'ko-','MarkerIndices',1:7:181,'MarkerFaceColor','k','MarkerSize',markerSize)
% plot(target_latitude,1.6*twosat_save{ii,1},'kd-','MarkerIndices',1:7:181,'MarkerFaceColor','k','MarkerSize',markerSize)
% plot(target_latitude,1.6*0.75*threesat_save{ii,1},'ks-','MarkerIndices',1:7:181,'MarkerFaceColor','k','MarkerSize',markerSize)
% text_list = sprintf('θ = %d',CameraAngleCapability(ii));
% 
% end
% xlabel('Latitude [Deg]')
% ylabel('Sunspot visibility per solar rotation [Days]')
% set(gca,'fontsize',15)
% xlim([-90,90])
% for ii = 1
% plot(target_latitude,1.6*onesat_save{ii,2},'ro-','MarkerIndices',1:7:181,'MarkerFaceColor','r','MarkerSize',markerSize)
% plot(target_latitude,1.6*twosat_save{ii,2},'rd-','MarkerIndices',1:7:181,'MarkerFaceColor','r','MarkerSize',markerSize)
% plot(target_latitude,1.6*0.75*threesat_save{ii,2},'rs-','MarkerIndices',1:7:181,'MarkerFaceColor','r','MarkerSize',markerSize)
% text_list = sprintf('θ = %d',CameraAngleCapability(ii));
% 
% end
% legend('1-sat [14.5deg]','2-sat [14.5deg]','3-sat [14.5deg]','1-sat [0deg]','2-sat [0deg]','3-sat [0deg]')
% axis square

%%

markerSize = 4;
figure;
hold on;
x=-10;
for ii = 1

text_list = sprintf('θ = %d',CameraAngleCapability(ii));

end
ylabel('Heliographic Latitude [deg]','Interpreter','latex')
xlabel('Sunspot visibility per solar rotation [Days]','Interpreter','latex')

ylim([-90,90])
for ii = 1

text_list = sprintf('θ = %d',CameraAngleCapability(ii));
end

plot(1.6*onesat_save{ii,2},target_latitude,'k-','MarkerIndices',1:7:181,'MarkerFaceColor','k','MarkerSize',markerSize)
plot(1.6*twosat_save{ii,2},target_latitude,'kd-','MarkerIndices',1:7:181,'MarkerFaceColor','k','MarkerSize',markerSize)
plot(1.6*0.75*threesat_save{ii,2},target_latitude,'ks-','MarkerIndices',1:7:181,'MarkerFaceColor','k','MarkerSize',markerSize)
plot(1.6*twosat_save{ii,1},target_latitude,'rd-','MarkerIndices',1:7:181,'MarkerFaceColor','r','MarkerSize',markerSize)
plot(1.6*0.75*threesat_save{ii,1},target_latitude,'rs-','MarkerIndices',1:7:181,'MarkerFaceColor','r','MarkerSize',markerSize)

legend('L1 [0\circ]','L1+L4 [0\circ]','L1+L4+L5 [0\circ]','L1+L4 [14.5\circ]','L1+L4+L5 [14.5\circ]','Interpreter','tex','fontname','times new roman')
set(gca,'fontsize',12)

axis square